package co.edu.unal.cg.splines;
import java.util.ArrayList;

import org.la4j.linear.LinearSystem;
import org.la4j.matrix.Matrices;
import org.la4j.matrix.Matrix;
import org.la4j.matrix.dense.Basic2DMatrix;
import org.la4j.matrix.dense.DenseSafeMatrix;
import org.la4j.matrix.sparse.CRSMatrix;
import org.la4j.vector.Vector;
import org.la4j.vector.dense.BasicVector;

import processing.core.PApplet;
import processing.core.PVector;

public class NaturalSpline extends Spline{

	double stepSize;
	ArrayList<PVector> controlPoints = new ArrayList<PVector>();
	
	public NaturalSpline(){
		super(0.1);
	}
	
	public void addControlPoint(PVector p){
		this.controlPoints.add(p);
	}

	public NaturalSpline(double stepSize) {
		super(stepSize);
	}

	public ArrayList<Matrix> getCoefficients() {
		
		int n = controlPoints.size() - 1; // Number of segments
		ArrayList<Matrix> coefficients = new ArrayList<Matrix>();
		if(controlPoints.size()<3) return coefficients;
		Matrix a = buildMatrix();
		Vector b = buildB();
		
		LinearSystem system = Matrices.asLinearSystem(a, b);
		Vector x = system.solve();

		

		for (int i = 0; i < n; i++) {
			coefficients.add(i, new Basic2DMatrix(3,4));
			for (int j = 0; j < 3; j++) {
				for (int k = 0; k < 4; k++) {
					coefficients.get(i).set(j,k,x.get((i * 12) + (j * 4) + k));
				}
			}
		}
		return coefficients;
	}

	private Vector buildB() {
		int n = controlPoints.size() - 1;
		Vector b = new BasicVector(3* 4 * n);
		b.assign(0.0d);
		PVector p = controlPoints.get(0);
		b.set(0, p.x);
		b.set(1, p.y);
		b.set(2, p.z);
		int i;
		for (i = 1; i < n; i++) {
			p = controlPoints.get(i);
			b.set(i * 6 + 0 -3 , p.x);
			b.set(i * 6 + 1 -3, p.y);
			b.set(i * 6 + 2 -3, p.z);
			b.set(i * 6 + 3 -3, p.x);
			b.set(i * 6 + 4 -3, p.y);
			b.set(i * 6 + 5 -3, p.z);
		}
		p = controlPoints.get(n);
		b.set(i * 6 + 0 - 3, p.x);
		b.set(i * 6 + 1 - 3, p.y);
		b.set(i * 6 + 2 - 3, p.z);
		return b;
	}

	private Matrix buildMatrix() {

		int n = controlPoints.size() - 1;
		Matrix A = new CRSMatrix(3 * 4 * n, 3 * 4 * n);
		A = A.safe();

		for (int i = 0; i < n; i++) {
			for (int j = 0; j < 3; j++) {
				fillU(A, (i * 6) + j, (i * 3 * 4) + (j * 4), 0);
				fillU(A, (i * 6) + j + 3, (i * 3 * 4) + (j * 4), 1);
			}
		}

		for (int i = 0; i < n - 1; i++) {
			for (int j = 0; j < 3; j++) {
				filldU(A, 6 * n + (i * 6) + j, (i * 3 * 4) + (j * 4), 1, -1);
				filldU(A, 6 * n + (i * 6) + j, (i * 3 * 4) + (j * 4) + 12, 0,
						1);

				fillddU(A, 6 * n + (i * 6) + j + 3, (i * 3 * 4) + (j * 4), 1,
						-1);
				fillddU(A, 6 * n + (i * 6) + j + 3,
						(i * 3 * 4) + (j * 4) + 12, 0, 1);

			}
		}
		for (int j = 0; j < 3; j++) {
			fillddU(A, 12 * n - 6 + j, j * 4, 0, 1);
		}
		for (int j = 0; j < 3; j++) {
			fillddU(A, 12 * n - 3 + j, 3 * 4 * (n - 1) + 4*j, 1, 1);
		}

		return A;
	}

	private static void fillU(Matrix M, int i, int j, double u) {
		for (int k = j; k < j + 4; k++)
			M.set(i, k, Math.pow(u, 3 - k + j));
	}

	private static void filldU(Matrix M, int i, int j, double u, double scale) {
		for (int k = j; k < j + 3; k++)
			M.set(i, k, (3 - k + j) * Math.pow(u, 2 - k + j) * scale);
	}

	private static void fillddU(Matrix M, int i, int j, double u, double scale) {
		for (int k = j; k < j + 2; k++)
			M.set(i, k, (2 - k + j) * (3 - k + j) * Math.pow(u, 1 - k + j)
					* scale);
	}
}
